
(ns pathfinder-algorithm
  (:require
    [clojure.contrib.str-utils2 :as su])
  ;(:use  )
  ;(:import )
  )

;open list for checking nodes
(def open-list nil)
;closed list for nodes that no longer should be checked, candidates for closest path
(def closed-list [])
;finish node position
(def f-position [0 0])
;start node position
(def s-position [0 0])
;playground map
(def playground-map {})
;state can be nsf (no start or finish position), nr (no result) or empty string
;if path is found
(def state "")

(defn initialize-all [pg-map]
  (def open-list nil)
  (def closed-list [])
  (def f-position [0 0])
  (def s-position [0 0])
  (def playground-map pg-map)
  (def state "")
  )

;sets finish node position
(defn set-f-position [f-node]
  (def f-position (nth f-node 0))
  )

;sets start  node position
(defn set-s-position [s-node]
  (def s-position (nth s-node 0))
  )

;assigns parent to node
(defn assign-parent [node parent-node]
  (assoc node :parent parent-node)
  )

;gets parent
(defn get-parent [node1]
  (if (nil? node1)
    nil
    (if(nil? (find (val node1) :parent))
      nil;then
      (val (find (val node1) :parent)));else
    )
  )

;finds position of special node (only for start and finish node)
(defn get-special-node-position [status]
  (for [node1 playground-map
        :when (= (val (find (val node1) :status)) (su/lower-case status))]
    (key node1)
    )
  )

;calculate f value for node
(defn calculate-f [node-value]
  (if-not (nil? node-value )
    (let [g (val (find (val node-value) :g))
          h (val (find (val node-value) :h))]
      (def playground-map (assoc playground-map (key node-value) (assoc (val node-value) :f (+ g h))))
       (find  playground-map (key node-value))
      )
  )
)

;define heuristics adder value
(defn get-adder-value [against-node-position for-node-position]
  (if (= (get against-node-position 0) (get for-node-position 0)) ;sa istim x
    10
    (if (= (get against-node-position 1) (get for-node-position 1)) ;sa istim y
      10
      14 ; po dijagonali
      )
    )
  )

;calculates g value for node
;it depends on position of for-node to against-node
;if it is on diagonal position on g of against-node is added 14
;othervise adds 10
;assign parent for node
(defn calculate-g [against-node for-node]
  (let [against-node-position (key against-node)
        for-node-position (key for-node)
        against-node-g (val(find (val against-node) :g))
        for-node-g (val(find (val for-node) :g))]

    (let [adder (get-adder-value against-node-position for-node-position)
          g-candidate (+ against-node-g adder)]
      (if-not (nil? (val (find (val for-node) :parent)))
        (if (< g-candidate for-node-g)
          ;A* algorithm - change parent if current node is better parent
          (let [node-val-with-g (assoc (val for-node) :g g-candidate)
                node-value (assign-parent node-val-with-g against-node)]
            (def playground-map (assoc playground-map (key for-node) node-value))

            )
          )
        (let [node-val-with-g (assoc (val for-node) :g g-candidate)
              node-value (assign-parent node-val-with-g against-node)]
          (def playground-map (assoc playground-map (key for-node) node-value))

          );then
        )
      )

    )
  (find playground-map (key for-node))
  )


;calculates h value for node
(defn calculate-h [node1]
  (let [node-x-position (get (key node1) 0)
        node-y-position (get (key node1) 1)
        f-x-position (get f-position 0)
        f-y-position (get f-position 1)
        x-distinct (Math/abs (- node-x-position f-x-position))
        y-distinct (Math/abs (- node-y-position f-y-position))
        h (Math/sqrt (+ (Math/pow x-distinct 2) (Math/pow y-distinct 2)))]
    (def playground-map (assoc playground-map (key node1) (assoc (val node1) :h (* h 10))))
    (find playground-map (key node1))
    )
  )

;check if node is already in open list
(defn node-is-in-open-list [node1]
  (not (empty? (for [n open-list
                     :when (= (key n) (key node1))]
                 n
                 )
         )
    )
  )

(defn node-is-in-closed-list [node1]
  (not (empty? (for [n closed-list
                     :when (= (key n) (key node1))]
                 n
                 )
         )
    )
  )

;check if node can be added to open list
(defn node-can-be-added-to-open-list [node1]
  (if-not (nil? node1)
    (and (not (node-is-in-open-list node1))
      (not (= (val (find (val node1) :status)) "notempty"))
      )
    false
    )
  )

;removes node from open list to move it from closed
(defn remove-node-from-open-list [node1]
  (def open-list (remove #(= (key %) (key node1)) open-list))
  (def open-list (sort-by #((val %) :f) open-list))
  )

;puts node to open list if it is needed (if nod is not already in open list
;and if node is not nil and if node is empty)
;and calculates all node mesures
(defn put-node-to-open-list [node1 against-node]
  (if (and (not (node-is-in-closed-list node1))
        (not (= (val (find (val node1) :status)) "notempty"))
        )
    (if(node-is-in-open-list node1)
      (dosync
        (remove-node-from-open-list node1)
        (let [tmp-node (calculate-f (calculate-g against-node node1))]
          (def open-list (conj open-list tmp-node))
          )
        )
      (let [tmp-node (calculate-f (calculate-g against-node (calculate-h node1)))]
        (def open-list (conj open-list tmp-node))
        )
      )
    )
  )

;adds surrounding nodes of start-node to open list
;only if nodes can be added
(defn add-nodes-to-open-list [start-node]
  (let [start-node-x-position (get (key start-node) 0)
        start-node-y-position (get (key start-node) 1)]
    ;diagonal up left
    (put-node-to-open-list (find playground-map
                             [(- start-node-x-position 1) (- start-node-y-position 1)])
      start-node
      )
    ;above
    (put-node-to-open-list (find playground-map
                             [ start-node-x-position (- start-node-y-position 1)])
      start-node
      )
    ;diagonal up right
    (put-node-to-open-list (find playground-map
                             [ (+ start-node-x-position 1) (- start-node-y-position 1)])
      start-node
      )
    ;left
    (put-node-to-open-list (find playground-map
                             [ (- start-node-x-position 1) start-node-y-position ])
      start-node
      )
    ;rigth
    (put-node-to-open-list (find playground-map
                             [ (+ start-node-x-position 1) start-node-y-position ])
      start-node
      )
    ;diagonal down left
    (put-node-to-open-list (find playground-map
                             [ (- start-node-x-position 1) (+ start-node-y-position 1)])
      start-node
      )
    ;below
    (put-node-to-open-list (find playground-map
                             [ start-node-x-position (+ start-node-y-position 1)])
      start-node
      )
    ;diagonal down right
    (put-node-to-open-list (find playground-map
                             [ (+ start-node-x-position 1) (+ start-node-y-position 1)])
      start-node
      )
    )
  (def open-list (sort-by #((val %) :f) open-list))
  )

;adds node to closed list
(defn add-node-to-closed-list [node1]
  (if-not (nil? node1)
    (def closed-list (conj closed-list node1))
    )
  )

;finds next node with smallest f
(defn move-node-to-closed-list [node1]
  (add-node-to-closed-list node1)
  (add-nodes-to-open-list node1)
  (remove-node-from-open-list node1)
  )

(defn set-path [f-node]
  (def parent-node (get-parent f-node))
  (if-not (= (key parent-node) s-position)
    (def playground-map (assoc playground-map (key parent-node) (assoc (val parent-node) :status "x")))
    )
  (def parent-node (find
                     playground-map
                     (key parent-node)))
  (if-not (= (key parent-node) s-position)
    (recur parent-node)
    )
  )

(defn do-path [node1]
  (move-node-to-closed-list node1)
  (let [node-with-smallest-f (first open-list)]
    (if-not (nil? node-with-smallest-f)
      (if (= (key node-with-smallest-f) f-position)
        (set-path node-with-smallest-f);findpath
        (if-not (empty? open-list)
          (recur node-with-smallest-f)
          )
        )
      (def state "np")
      )
    )
  )

(defn find-path [pg-map]

  ;(set-playground-map pathfinder/matrix)
  (initialize-all pg-map)

  (let [f (get-special-node-position  "f")
        s (get-special-node-position  "s")]
    (if (or (empty? f) (empty? s))
      (def state "nsf")
      (dosync
        (set-f-position f)
        (set-s-position s)
        (let [start-node (find playground-map (nth s 0))]

          (let [newSeq #{start-node}]
            (def open-list (seq newSeq))
            ;(def open-list (seq #{(find playground-map s-position)}))

            (do-path (first open-list))
            
            )
          )
        )
      )
    )
  )